#include "power_board_pkg/simple_dataframe_master.h"
#include "power_board_pkg/base_driver.h"
#include "power_board_pkg/m_utility_fun.h"

// #include "transport.h"
// Simple_dataframe::Simple_dataframe(Transport* _trans): trans(_trans){
//     recv_state = STATE_RECV_FIX;
// }

Simple_dataframe* Simple_dataframe::FrameInstance = NULL;
rclcpp::Node::SharedPtr Simple_dataframe::node= NULL;

Simple_dataframe::Simple_dataframe(){
    recv_state = STATE_RECV_FIX;

}
Simple_dataframe::~Simple_dataframe(){
}

bool Simple_dataframe::send_message(const MESSAGE_ID id){
    Message msg(id);

    send_message(&msg);

    return true;
}

bool Simple_dataframe::send_message(const MESSAGE_ID id, const unsigned char* data, const unsigned char len){
    Message msg(id, data, len);

    send_message(&msg);

    return true;
}

bool Simple_dataframe::send_message(Message* msg){
    unsigned char* buf = (unsigned char*)msg;   
    int len = sizeof(msg->head) + msg->head.length + 1; 
    int ret = Tcp_Talk::Tcp_Instance()->Tcp_Client_Write(buf,len);
    // for(int i = 0;i < len;i ++)
    //     printf("%02x ",buf[i]);
    // printf("\n");
    return true;
}

bool Simple_dataframe::init(){
    return true;
}

bool Simple_dataframe::interact(const MESSAGE_ID id,const unsigned char* data,const char len)
{ 
    send_message(id,data,len);

    return true;
}

bool Simple_dataframe::recv_proc(){
    unsigned char buf[MAXSIZE];
    int len;
    int ret;
    memset(buf,0,MAXSIZE);
    len = Tcp_Talk::Tcp_Instance()->Tcp_Client_Read(buf);
    if(len > 0)
    {
        for (int i=0;i<len;i++){
            if (data_recv(buf[i])){
                data_parse();
            }
        }
    }
    else 
    {
        return false;
    }

    return true;
}

bool Simple_dataframe::data_recv(unsigned char c){
    switch (recv_state){
    case STATE_RECV_FIX:
        if (c == FIX_HEAD){
            memset(&active_rx_msg,0, sizeof(active_rx_msg));
            active_rx_msg.head.flag = c;
            active_rx_msg.check += c;
            recv_state = STATE_RECV_ID1;
        }
        else
            recv_state = STATE_RECV_FIX;
        break;
    case STATE_RECV_ID1:
				active_rx_msg.head.msg_id = c;
				active_rx_msg.check += c;
				recv_state = STATE_RECV_ID2;

        break;
		case STATE_RECV_ID2:
				active_rx_msg.head.msg_id |= c << 8;
				active_rx_msg.check += c;
				if(active_rx_msg.head.msg_id == MES_ID)
					recv_state = STATE_RECV_LEN;
				else
					recv_state = STATE_RECV_FIX;
        break;
        case STATE_RECV_LEN:
                active_rx_msg.head.length = c;
                active_rx_msg.check += c;
				recv_state = STATE_MAIN_CMD;
        break;
				
		case STATE_MAIN_CMD:
				active_rx_msg.rdata.cmd = c << 8;
				active_rx_msg.check += c;
				recv_state = STATE_SEED_CMD;
				break;
		
		case STATE_SEED_CMD:
				active_rx_msg.rdata.cmd |= c ;
				active_rx_msg.check += c;
				if(active_rx_msg.head.length - 2 > 0)
					recv_state = STATE_RECV_DATA;
				else
					recv_state = STATE_RECV_CHECK;
				break;
    case STATE_RECV_DATA:
        active_rx_msg.rdata.data[active_rx_msg.recv_count++] = c;
        active_rx_msg.check += c;
        if (active_rx_msg.recv_count >= active_rx_msg.head.length - 2)
            recv_state  = STATE_RECV_CHECK;
        break;
    case STATE_RECV_CHECK:
        recv_state = STATE_RECV_FIX;
        if (active_rx_msg.check == c){
            return true;
        }
        break;
    default:
        recv_state = STATE_RECV_FIX;
		break;
    }

    return false;
}

bool Simple_dataframe::data_parse(){
    MESSAGE_ID id = (MESSAGE_ID)active_rx_msg.rdata.cmd;
    unsigned char* data = active_rx_msg.rdata.data;
    int length = active_rx_msg.head.length;
    Data_holder* dh = Data_holder::get();

    std_msgs::msg::String versionpub;
    char softversion[5] = {0};
    // printf("active_rx_msg.rdata.cmd = %x\n",active_rx_msg.rdata.cmd);
    switch (id){
		case GET_POWER_BTN_STATE:
            // printf("btn data = %d\n",data[0]);
            dh->power_board.btn = data[0];
            if(data[0] == 0x01)
            {
                RCLCPP_INFO(node->get_logger(),"POWER_BTN_STATE!\n");
                dh->power_board.status |= (1 << 0);
                dh->Flag._shutdownflag = true;
                // printf(" dh->power_board.status = %d\n", dh->power_board.status);
                // printf("dh->Flag._shutdownflag  = %d\n",dh->Flag._shutdownflag);
            }
            dh->Flag._btndataupdate = true;
		break;

        case GET_BAT_STATE:
            if(data[0] == 0x01)
            {
                dh->power_board.status |= (1 << 1);
            }
            else
            {
                dh->power_board.status &= ~(1 << 1);
            }
        break;
        case GET_CHARGE_STATE:
        {
			dh->batterydata.chargestatus = data[0];
			dh->Flag._chargestatusupdate = true;
        }
        break;
        case SET_GET_FIRMWARE_VERSION: //yyy 2020-9-18 v3.1.0
            HexToStr(softversion, (const char*)&data[2], 2);
            versionpub.data = softversion;
            BaseDriver::Instance()->pb_version_pub->publish(versionpub);
            // printf("version[0] = %04x\n",versionpub.data.at(0));
            // printf("version[1] = %04x\n",versionpub.data.at(1));

            // printf("dh->power_board.hardwareversion = %04x\n",dh->power_board.hardwareversion);
            // printf("dh->power_board.softwareversion = %04x\n",dh->power_board.softwareversion);
        break;
        case GET_BAT_MES2:
            memcpy(&dh->batteryinfo,data,(length - 2));
            dh->Flag._batteryupdate = true;
            // printf("dh->batteryinfo.soh = %d\n",dh->batteryinfo.soh);
        break;
		default:
		break;
    }	
	return true;
}






